#include <utils.h>
#include <string.h>
#include <io.h>

int Abs(int x) {
  return (x >= 0 ? x : -x);
}

int Min(int a, int b) {
  return (a < b ? a : b);
}

int Max(int a, int b) {
  return (a > b ? a : b);
}

void WakeAwaiters( sched *s, td *task, int eventid, int ret,
    char *event, int eventlen) {
  while (!PqEmpty(&s->waiters[eventid])) {
    td *next_task = PqPopFront(&s->waiters[eventid]);
    next_task->ret = ret;
    next_task->state = STATE_READY;
    // copy data over
    int len = Min(next_task->event_len, eventlen);
    if (next_task->event_len < len)
      Panic("WakeAwaiters", "not enough room for event data in awaiter");
    MemCopy(next_task->event, event, len);
    PqPushBack( &s->queues[MAX_PRI], next_task );
  }
}

void SensorToExternal(char *sensor_data, char *external) {
  int set, bit;
  for (set = 0; set < 5; set++) {
    char b1 = sensor_data[2*set], b2 = sensor_data[2*set+1];
    unsigned short val = (b1 << 8) | b2;
    if (val == 0)
      continue;
    for (bit = 1; bit <= 16; bit++) {
      if (val & (1 << (16-bit))) {
        int len = SPrintf(external, "%c%d", set+'A', bit);
        external[len] = '\0';
        return;
      }
    }
  }
}

int ExternalToInternal(char *external) {
  if (external[0] == 'D' && external[1] == 'E') {
    // stop
    int id = Atoi(external+2);
    return 61+id;
  }
  if (external[0] >= 'A' && external[0] <= 'E') {
    // sensor
    int c = external[0]-'A';
    int b = Atoi(external+1);
    return 8*c+((b-1)/2);
  }
  // switch
  if (external[3] >= 'A')
    return 58+external[3]-'A';
  int sw = Atoi(external+2);
  if (sw == 99)
    return 61;
  return sw+39;
}

void InternalToExternal(int internal, char *external) {
  int len;
  if (internal < 40) {
    // sensor
    char set = 'A'+(internal/8);
    int bit = (internal%8)*2+1;
    len = SPrintf(external, "%c%d", set, bit);
  } else if (internal < 58) {
    // switch 1-18
    len = SPrintf(external, "SW%d", internal-39);
  } else if (internal < 61) {
    // switch 9A-9C
    len = SPrintf(external, "SW9%c", internal-58+'A');
  } else if (internal == 61) {
    len = SPrintf(external, "SW99");
  } else {
    len = SPrintf(external, "DE%d", internal-61);
  }
  external[len] = '\0';
}

int SwitchToInternal(int switch_number) {
  if (switch_number <= 18)
    return 39+switch_number;
  if (switch_number == 0x99)
    return 61;
  return 58+switch_number-0x9a;
}

int InternalToSwitch(int internal) {
  if (40 <= internal && internal < 58)
    return internal-39;
  if (internal == 61)
    return 0x99;
  return 0x9a+internal-58;
}
